Skip to content


ai  101  pytorch  classification  nvidia  cuda  install  tensorrt  yolo  ardupilot  None  ros2  dds  micro ros  xrce  sitl  plugin  SITL  debug  rangefinder  pymavlink  mavros  gazebo  distance sensor  system_time  timesync  cmake  gtest  ctest  cpp  c++  format  fmt  multithreading  spdlog  camera  coordinate system  orb  matching  opencv  build  transformation  computer vision  homography  optical flow  of  trackers  cv  cyclonedds  eprosima  fastdds  simulation  config  ignition  bridge  sdf  tips  ign-transport  sensors  lidar  aptly  apt  encryption  pgp  docker  git  bundle  github  hooks  pre-commit  lxd  container  lxc  x11  profile  vscode  marpit  presentation  marp  markdown  mermaid  video  ffmpeg  gstreamer  cheat-sheet  sdp  v4l2loopback  gi  snippets  cheat Sheet  python  asyncio  future  click  cli  numpy  project  template  black  isort  docs  project document  docstrings  flake8  linter  git-hook  mypy  unittest  pytest  pylint  mock  iterator  generator  logging  tuple  namedtuple  typing  annotation  pyzmq  zmq  msgpack  action  namespace  remap  control2  ros2_control  gdb  qos  tag  plugins  msg  node  zero-copy  shm  tutorial  algorithm  calibration  diff  pid  dev  colcon  colcon_cd  rpi  arm  qemu  settings  behavior  plot  visualization  debugging  diagnostic  diagnostics  tutorials  gst  math  apm  rat_runtime_monitor  web  rosbridge  vue  binding  discovery  gazebo-classic  launch  spawn  cook  gps  imu  ray  gazebo_ros_ray_sensor  ultrsonic  range  ultrasonic  gazebo classic  wrench  effort  odom  ign  gz  xacro  ros_ign  diff_drive  odometry  joint_state  argument  OpaqueFunction  DeclareLaunchArgument  LaunchConfiguration  tmux  nav  slam  test  rclpy  executor  MultiThreadedExecutor  SingleThreadedExecutor  param  dynamic-reconfigure  service  client  setup.py  package.xml  parameter  parameters  custom  msgs  executers  pub  sub  rqt  rviz  rviz2  pose  marker  tf2  deb  package  setup  local_setup  rosdep  package manager  project settings  vcstool  cross-compiler  nano  texture  tmuxp  rootfs  embedded  zah  linux  rm  ubuntu  ip  ss  network  netstat  snap  deploy  ssh  systemd  mkdocs  extensions  socat  networking  serial  udp  tc  mtu  select  px4  robotics  kalman_filter  kalman  filter  control  todo  vscode-ext  json  yaml  schema  yocto  poky  world  gazebo_ros2_control  position_controller  effort_controller  velocity_controller  urdf  gazebo_ros_force  gazebo_ros_joint_state_publisher  robot_state_publisher  joint_state_publisher  projects  vrx  buoyancy 

ROS2 gazebo (classic) ultrasonic sensor


  • Create simple model with ultrasonic sensor
  • Spawn the model into gazebo world
  • View sensor output in RVIZ

sdf / sensor#

ultrasonic/model.sdf
<?xml version="1.0"?>
<sdf version="1.5">
    <model name="ultrasonic">
        <static>true</static>
        <link name="link">
            <inertial>
                <mass>0.1</mass>
                <inertia>
                  <ixx>0.000166667</ixx>
                  <iyy>0.000166667</iyy>
                  <izz>0.000166667</izz>
                </inertia>
              </inertial>
            <collision name="collision">
                <geometry>
                    <box>
                        <size>0.1 0.1 0.1</size>
                    </box>
                </geometry>
            </collision>
            <visual name="visual">
                <geometry>
                    <box>
                        <size>0.1 0.1 0.1</size>
                    </box>
                </geometry>
            </visual>
            <sensor name="ultrasonic_1" type="ray">
                <pose>0.05 0 0 0 0 0</pose>
                <always_on>true</always_on>
                <visualize>true</visualize>
                <update_rate>10</update_rate>
                <ray>
                    <scan>
                        <horizontal>
                            <samples>5</samples>
                            <resolution>1.000000</resolution>
                            <min_angle>-0.12</min_angle>
                            <max_angle>0.12</max_angle>
                        </horizontal>
                        <vertical>
                            <samples>5</samples>
                            <resolution>1.000000</resolution>
                            <min_angle>-0.01</min_angle>
                            <max_angle>0.01</max_angle>
                        </vertical>
                    </scan>
                    <range>
                        <min>0.01</min>
                        <max>4</max>
                        <resolution>0.01</resolution>
                    </range>
                    <noise>
                        <type>gaussian</type>
                        <mean>0.0</mean>
                        <stddev>0.01</stddev>
                    </noise>
                </ray>
                <plugin name="ultrasonic_sensor" filename="libgazebo_ros_ray_sensor.so">
                    <ros>
                        <remapping>~/out:=ultrasonic_sensor_1</remapping>
                    </ros>
                    <output_type>sensor_msgs/Range</output_type>
                    <radiation_type>ultrasound</radiation_type>
                    <frame_name>link</frame_name>
                </plugin>
            </sensor>
        </link>
    </model>
</sdf>

launch#

launch/ultrasonic.launch.py
import os
from launch import LaunchDescription
from launch.actions import AppendEnvironmentVariable, IncludeLaunchDescription, DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
import xacro

PACKAGE_NAME = "ros2_gazebo_tutorial"
WORLD = "camera.world"
MODEL = "ultrasonic"
SDF = "model.sdf"

def generate_launch_description():
    pkg = get_package_share_directory(PACKAGE_NAME)
    pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
    model_sdf_full_path = os.path.join(pkg, "models", MODEL, "model.sdf")

    sim_time = LaunchConfiguration("sim_time")
    arg_sim_time = DeclareLaunchArgument("sim_time", default_value="true")

    resources = [
        os.path.join(pkg, "worlds")    
    ]

    resource_env = AppendEnvironmentVariable(name="GAZEBO_RESOURCE_PATH", value=":".join(resources))

    start_gazebo_server_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
        launch_arguments={
            "verbose": "true", 
            'world': WORLD}.items())

    start_gazebo_client_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')))

    robot_description_path = os.path.join(pkg, "models", MODEL, SDF)
    doc = xacro.parse(open(robot_description_path))
    xacro.process_doc(doc)

    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[
            {
                'use_sim_time': sim_time, 
                'robot_description': doc.toxml()
            }
        ]
    )

    spawn_entity_cmd = Node(
        package="gazebo_ros", 
        executable="spawn_entity.py",
        arguments=['-entity', "robot_name_in_model", 
        '-file', model_sdf_full_path,
        '-x', "0",
        '-y', "0",
        '-z', "0.05"],
        output='screen')

    rviz = Node(
        package="rviz2",
        executable="rviz2",
        arguments=["-d", os.path.join(pkg, "config", "ultrasonic.rviz")],
    )

    link_tf = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name="link2world",
        arguments = ["0", "0", "0.05", "0", "0", "0", "world", "link"]
    )

    ld = LaunchDescription()
    ld.add_action(arg_sim_time)
    ld.add_action(resource_env)
    ld.add_action(start_gazebo_server_cmd)
    ld.add_action(start_gazebo_client_cmd)
    ld.add_action(robot_state_publisher)
    ld.add_action(spawn_entity_cmd)
    ld.add_action(rviz)
    ld.add_action(link_tf)
    return ld

tf#

  • Add static tf between world to link (model)
link_tf = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name="link2world",
        arguments = ["0", "0", "0.05", "0", "0", "0", "world", "link"]
    )

Test sensor read#

ultrasonic_demo_.py
#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Range
from rclpy.qos import qos_profile_sensor_data

TOPIC_RANGE = "/ultrasonic_sensor_1"

class MyNode(Node):
    def __init__(self):
        node_name="ultrasonic_demo"
        super().__init__(node_name)
        self.create_subscription(Range, TOPIC_RANGE, self.__range_handler, qos_profile=qos_profile_sensor_data)
        self.get_logger().info("Hello ultrasonic")

    def __range_handler(self, msg: Range):
        self.get_logger().info(str(msg.range))

def main(args=None):
    rclpy.init(args=args)
    node = MyNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

run#


run again#

out of range reading

When sensor reading is out of range:
- The visual marker turn brighter
- Rviz stop show marker
- BUG: subscriber stop read data from topic (for output_type sensor_msgs/Range)

echo topic
ros2 topic echo /ultrasonic_sensor_1 
Unable to convert call argument to Python object (compile in debug mode for details)